/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package teamBlitz;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Ultrasonic;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Random;
import java.util.TimerTask;

/**
 *
 * @author tlemmons
 */
public class Court {

    private static final short m_Width = 16459;
    private static final short m_Height = 8230;
    private static final short m_BridgeWidth = 2235;
    private static final short m_BridgeHeight = 1219;
    private static final short m_goalOffset = 695;
    private static final short m_goalHeight = 1;
    private java.util.Timer m_controlLoop;
    private int sonarCount=0;
    private final Robot2012 mRobot2012;

    public void init() {
    }

    private void CheckSonar() {
        this.sonicFwd.setEnabled(true);
        this.sonicLeft.setEnabled(true);
//        this.sonicFwd.ping();
        double range = this.sonicFwd.getRangeMM();
        SmartDashboard.log(range, "range foward");
//        System.out.println("range =" + range);
        range = this.sonicLeft.getRangeMM();
        SmartDashboard.log(range, "range Left");
    }

    public class state {

        public double m_XAccel = 0;
        public double m_YAccel = 0;
        public short m_X = 8230;
        public short m_Y = 412;
        public double m_Heading = 0;
        private double m_XVelocity = 0;
        private double m_YVelocity = 0;
    }
    private static final double Sample_Time_ms = 100;
    private static state m_Current;
    private static final Object m_currentLocationLock = new Object();
    private static float m_homeDirection = 0;  // starting direction to front of court
    ADXL345_I2C_6DOF accelarometer;
    ITG3200_I2C_6DOF gyro;
    MAG3110 magnetometer;
    HMC6352 compass;
    Ultrasonic sonicFwd;
    Ultrasonic sonicLeft;
//    Ultrasonic sonicRight;
    Target myTargets;
    double distanceFromBasket = 0;

// in robotInit get or create singleton instance (getTable will get instance if it exists or create one if it does not.
    public Court(Robot2012 robot2012) {
        //System.out.println("Court analysis started");
        mRobot2012=robot2012;
        m_Current = new state();
        myTargets = new Target();
        m_Current.m_X = m_Width / 2;
        m_Current.m_Y = m_Height / 2;
        Random a = new Random();
 //       SmartDashboard.putBoolean("Shooting", false);
        myTargets.addShot(1000 + 50, a.nextInt() % 4000, 0, a.nextInt() % 5);

//        for(int i=0;i<100;i++){
//            Random a=new Random();
//            
//myTargets.addShot(1000+i*50, a.nextInt()%4000, 0,a.nextInt()%5 );
//Timer.delay(.01);
//        }
        try {
            accelarometer = new ADXL345_I2C_6DOF(1, 2, ADXL345_I2C_6DOF.DataFormat_Range.k2G);
        } catch (InstantiationException ex) {
            ex.printStackTrace();
        }
        try {
            compass = new HMC6352(1);
            Timer.delay(1);
            m_homeDirection = compass.getHeading();
//          System.out.println("HEADING" + m_Current.m_Heading);
        } catch (InstantiationException ex) {
            ex.printStackTrace();
        }

        sonicFwd = new Ultrasonic(14, 13, Ultrasonic.Unit.kMillimeter);
        sonicLeft = new Ultrasonic(12, 11, Ultrasonic.Unit.kMillimeter);
        sonicFwd.setAutomaticMode(true);
        sonicLeft.setAutomaticMode(true);
//        sonicRight=new Ultrasonic(3,4,Ultrasonic.Unit.kMillimeter);
//         sonicRight.setAutomaticMode(true);

        // magnetometer=new MAG3110(1);
        // gyro=new ITG3200_I2C_6DOF(1);


        //  need to initialize where we are and home direction here

        //accelarometer.Calibrate();
        m_controlLoop = new java.util.Timer();
        m_controlLoop.schedule(new ChkTask(this), 0L, (long) (Sample_Time_ms));
        accelarometer.getAverages();
//        javax.microedition.io.SocketConnection sc;
//        try {
//            sc = (javax.microedition.io.SocketConnection) Connector.open("socket://10.20.83.101:1130");
//            sc.setSocketOption(javax.microedition.io.SocketConnection.LINGER, 5);
//
//            InputStream is = sc.openInputStream();
//            OutputStream os = sc.openOutputStream();
//            os.write("Testing\r\n".getBytes());
//            int ch = 0;
//            byte[] buffer = new byte[50];
//            while (ch != -1) {
//                ch = is.read(buffer);
//                String mydata = new String(buffer);
//            }
//
//            is.close();
//            os.close();
//            sc.close();
//        } catch (IOException ex) {
//            ex.printStackTrace();
//        }
//
        SmartDashboard.log(m_homeDirection, "Heading");
        SmartDashboard.log(m_Current.m_X, "location X");
        SmartDashboard.log(m_Current.m_Y, "location Y");
        SmartDashboard.log(m_Current.m_X | (m_Current.m_Y << 16), "location");
        SmartDashboard.log(0, "Acc X");
        SmartDashboard.log(0, "Acc Y");

    }

    private void update() {
        ADXL345_I2C_6DOF.AllAxes acc;
        //String serialIn;
        double distance;
        calculateLocation();

        //  acc=accelarometer.getAccelerations();
        // System.out.println("X="+m_Current.m_X);
        // Timer.delay(.5);
        //System.out.println();
    }

    public static state getLocation() {
        state tmp;
        synchronized (m_currentLocationLock) {
            tmp = m_Current;
        }
        return tmp;
    }

    public static double getBasketHeading() {
        double tmp;
        synchronized (m_currentLocationLock) {
            //TODO calculate heading
            int xB = m_Current.m_X - (m_Width / 2);
            int yB = m_Current.m_Y;

            tmp = MathUtils.atan2(xB, yB);
            tmp = Math.toDegrees(tmp);
        }
        return tmp;
    }

    public int getCurrentHeading() {
        int tmp;
        synchronized (m_currentLocationLock) {
            //TODO calculate heading
            tmp = (int) compass.getHeading();
        }
        return tmp;
    }

    private class ChkTask extends TimerTask {

        private Court m_me;

        public ChkTask(Court me) {
            if (me == null) {
                throw new NullPointerException("Given Court was null");
            }
            m_me = me;
        }

        public void run() {
            m_me.update();
        }
    }
    int count = 0;

    private synchronized void calculateLocation() {
        double aX, aY;
        double heading = Math.abs(m_homeDirection - compass.getAverageHeading());
        //heading=0;
        double mySin = Math.sin(Math.toRadians(heading));  // todo check this
        double myCos = Math.cos(Math.toRadians(heading));  // todo check this
        ADXL345_I2C_6DOF.AllAxes acc;
        try {
            int fire = NetworkTable.getTable("SmartDashboard").getInt("Shooting");
            if (fire!=0) {
                System.out.println("fire - bang="+fire);
                mRobot2012.Shoot(fire);
                //myTargets.addShot(distanceFromBasket, a.nextInt() % 4000, 0, a.nextInt() % 5);
                SmartDashboard.putInt("Shooting", 0);
            } else {
                //System.out.println("NOT fire - bang");
            }

        } catch (NetworkTableKeyNotDefined ex) {
            ex.printStackTrace();
        }

        acc = accelarometer.getAverages();

        aX = acc.XAxis / 3.0;
        aY = acc.YAxis / 3.0;

        double newX = (aX * myCos) + (aY * mySin); // calculate travel and convert to court diminsions
        double newY = (aY * myCos) + (aX * mySin);
        // dist=v * t

        m_Current.m_XAccel = newX * 9810;  // convert accel to cm/s^2
        m_Current.m_YAccel = newY * 9810;

        double XVelocity = m_Current.m_XAccel * (Sample_Time_ms / 1000); // multiply by time for velocity
        double YVelocity = m_Current.m_YAccel * (Sample_Time_ms / 1000); // multiply by time for velocity

        m_Current.m_XVelocity += XVelocity;
        m_Current.m_YVelocity += YVelocity;


        double distX = m_Current.m_XVelocity * (Sample_Time_ms / 1000); // multiply velocity by time for distance traveled
        double distY = m_Current.m_YVelocity * (Sample_Time_ms / 1000);


        if (aX == 0 && aY == 0) {
            if (count > 2) {
                distX = 0;
                distY = 0;
                m_Current.m_XVelocity = 0;
                m_Current.m_YVelocity = 0;
                count = 0;
            } else {
                count++;
            }

        }
        m_Current.m_X += distX * 3;
        m_Current.m_Y += distY * 3;
        if (m_Current.m_X < 0) {
            m_Current.m_X = 0;
        }
        if (m_Current.m_X > m_Width) {
            m_Current.m_X = m_Width;
        }
        if (m_Current.m_Y < 0) {
            m_Current.m_Y = 0;
        }
        if (m_Current.m_Y > m_Height) {
            m_Current.m_Y = m_Height;
        }
//            System.out.println(""+newX + "\t"+newY + "\t"+ m_Current.m_XVelocity+"\t"+(m_Current.m_X-(m_Width/2)));

        //distance to center of court basket
        double distanceFromBasket = Math.sqrt(Math.abs(m_Current.m_X - (m_Width / 2)) ^ 2) + ((m_Current.m_Y) ^ 2);
        SmartDashboard.log(distanceFromBasket, "distanceFromBasket");
        SmartDashboard.log(distanceFromBasket, "distanceFromBasket2");
        SmartDashboard.log(getBasketHeading(), "basketHeading");
        if (aX != 0 || aY != 0) {
//            System.out.println("LOCATION X=" + ((m_Current.m_X - (m_Width / 2)) / 25.6) + "   Y=" + ((m_Current.m_Y - (m_Height / 2)) / 25.6) + "  velocity X=" + m_Current.m_XVelocity + "   Y=" + m_Current.m_YVelocity);
//          System.out.println("  imACCELLERATION X=" + newX + "   Y=" + newY + "  aZ=" + acc.ZAxis);
//          System.out.println("  imvelocity X="+XVelocity+"   Y="+YVelocity);
//            System.out.println("velocity X=" + m_Current.m_XVelocity + "   Y=" + m_Current.m_YVelocity);
//            System.out.println("LOCATION X=" + ((m_Current.m_X-(m_Width/2))/25.6) + "   Y=" + ((m_Current.m_Y-(m_Height/2))/25.6)+"velocity X=" + m_Current.m_XVelocity + "   Y=" + m_Current.m_YVelocity);
//            System.out.println("");

//        System.out.println("HomeDirection="+m_homeDirection);
////        System.out.println("Direction="+compass.getDirection());
////        System.out.println("Compass="+compass.getHeading());
//            System.out.println("Heading=" + heading);
//            System.out.println("SINE=" + mySin);
//            System.out.println("COSINE=" + myCos);
//          System.out.println();

        }

        SmartDashboard.log(heading, "Heading");

//        m_Current.m_Y=m_Current.m_X;
        if (newX != 0 || newY != 0) {
            SmartDashboard.log(m_Current.m_X, "location X");
            SmartDashboard.log(m_Current.m_Y, "location Y");
            SmartDashboard.log(m_Current.m_X | (m_Current.m_Y << 16), "location");
        }
        Random a = new Random();
//        System.out.println("sent");
        SmartDashboard.log(acc.XAxis, "Acc X");
        SmartDashboard.log(acc.YAxis, "Acc Y");

        if (sonarCount++>=10 && Math.abs(heading) < 5) {
            CheckSonar();
            if(sonarCount>15){
            sonarCount=0;
            }
        } else {
            this.sonicFwd.setEnabled(false);
            this.sonicLeft.setEnabled(false);

        }

    }

    double getDistanceToBasket() {
        return distanceFromBasket;
    }

    double getPowerNeeded() {
        return myTargets.getPower(getDistanceToBasket());
    }
}
